"""
PyQt5版GUI工具
"""
from PyQt5.QtGui import QResizeEvent
from PyQt5.QtWidgets import *
from PyQt5.QtCore import *
from PyQt5.QtGui import *
from ui.Ui_main_window import Ui_MainWindow
import sys

from models.quadcopter import Quadcopter


def transform(ratio, from_value, to_value):
    return ratio * (to_value - from_value) + from_value

# 每秒100帧
FPS = 100

DEFAULT_KP = 80.0
DEFAULT_KD = 30.0
DEFAULT_KI = 0.01

# DEFAULT_KP = 15.0
# DEFAULT_KD = 800.0
# DEFAULT_KI = 0.10

class MainWindow(QMainWindow):

    def __init__(self):
        super().__init__()
        # 创建对象
        self.ui = Ui_MainWindow()
        # 初始化内容
        self.ui.setupUi(self)
        self.quadcopter_movie = QMovie(":/anim/quadcopter")
        self.pixel_height_offset = 40
        self.quadcopter = Quadcopter(initial_height=0)
        
        self.target_height = 0
        
        self.thrust = 0
        
        self.last_cte = 0
        self.cte_sum = 0
        
        self.k_p = DEFAULT_KP
        self.k_d = DEFAULT_KD
        self.k_i = DEFAULT_KI
        
        self.__init_ui()
        
        self.ui.slider_p.setValue(int(self.k_p * 10))
        self.ui.slider_d.setValue(int(self.k_d))
        self.ui.slider_i.setValue(int(self.k_i * 100))
        
        # self.update_target_line()
        
    def run(self):
        current = self.quadcopter.get_height()

        # ---------------------------- start
        
        # 目标值和当前值的差值 Proportional
        cte = self.target_height - current
        p = self.k_p * cte                      # p
        
        # 前两次误差的变化 Derivative
        d = self.k_d * (cte - self.last_cte)    # d
        self.last_cte = cte
        
        # 历史误差之和，消除稳态误差 Integral
        self.cte_sum += cte
        i = self.k_i * self.cte_sum             # i
        
        self.thrust = p + d + i
        # ---------------------------- end
        
        # print(f"current: {current:.3f}, target: {self.target_height:.3f}, cte: {cte:.3f}, thrust: {self.thrust:.3f}")
                
        self.quadcopter.fly(self.thrust, delta_t= 1 / FPS )
        
        self.update_target_line()
        self.update_quadcopter_height()
        
        
    def height_to_pixel(self, height):
        total_height = self.ui.frame_right.height()
        copter_height = self.ui.label_img.height()
        # transform [0, 100] -> [total_height - copter_height, 40]
        return transform(height / 10, total_height - copter_height * 0.5, self.pixel_height_offset )
    
    def pixel_to_height(self, pixel):
        total_height = self.ui.frame_right.height()
        copter_height = self.ui.label_img.height()
        # transform [total_height - copter_height, top] -> [0, 100]
        return transform((pixel - self.pixel_height_offset ) / (total_height - copter_height * 0.5 - self.pixel_height_offset ), 10, 0)
        
    def update_quadcopter_height(self):
        height = self.quadcopter.get_height()
        self.ui.rp_widget.update_plot(height)
        transformed_height = self.height_to_pixel(height)
        self.ui.label_img.move(0, int(transformed_height) - 30)
        
    def update_target_line(self):
        self.ui.line_target.move(0, int(self.height_to_pixel(self.target_height)))
        self.ui.rp_widget.update_target(self.target_height)
        
    def onFrameMousePress(self, event: QMouseEvent):
        if event.button() == Qt.LeftButton:
            # 获取鼠标位置
            mouse_position = event.pos()
            # print(mouse_position)
            self.target_height = self.pixel_to_height(mouse_position.y())
            
            # self.update_target_line()
            # self.quadcopter = Quadcopter(initial_height=height)
            
    def on_slider_value_changed(self, value, label):
        print(f"{label}: {value}")
        # kp-> max: 100
        if label == "p":
            self.k_p = value * 0.1
            self.ui.sb_p.setValue(self.k_p)
        elif label == "d":
            # kd-> max: 1000
            self.k_d = value
            self.ui.sb_d.setValue(self.k_d)
        elif label == "i":
            # ki-> max: 10
            self.k_i = value * 0.01
            self.ui.sb_i.setValue(self.k_i)
            
        
    def on_pid_reset(self):
        self.last_cte = 0
        self.cte_sum = 0
        
        self.k_p = DEFAULT_KP
        self.k_d = DEFAULT_KD
        self.k_i = DEFAULT_KI
        self.ui.slider_p.setValue(int(self.k_p * 10))
        self.ui.slider_d.setValue(int(self.k_d))
        self.ui.slider_i.setValue(int(self.k_i * 100))
        
        # self.quadcopter = Quadcopter(initial_height=0)
        self.target_height = 0
        self.update_target_line()
        
    def __init_ui(self):
        self.ui.slider_p.valueChanged.connect(lambda v: self.on_slider_value_changed(v, "p"))
        self.ui.slider_d.valueChanged.connect(lambda v: self.on_slider_value_changed(v, "d"))
        self.ui.slider_i.valueChanged.connect(lambda v: self.on_slider_value_changed(v, "i"))
        
        self.ui.btn_reset.clicked.connect(self.on_pid_reset)
        
        self.ui.frame_right.mousePressEvent = self.onFrameMousePress
        self.ui.label_img.setMovie(self.quadcopter_movie)
        self.quadcopter_movie.start()
        
def main():
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()
    
    # # 生成一个值在[0, 100]的随机数
    timer = QTimer()
    # timer.setInterval(100)
    timer.timeout.connect(lambda: window.run())
    timer.start(int(1000 / FPS))
    
    sys.exit(app.exec_())


if __name__ == '__main__':
    main()
